//!
/**
 ***********************************************************************************************************
 * <RBTransformSystem implementation>
 ***********************************************************************************************************
 * @file RBTransformSystem.cpp
 ***********************************************************************************************************/

/************************************************************************************************************
 * Includes
 ************************************************************************************************************/
#include "RBTransformSystem.h"

#include "RBTransformComponent.h"

/************************************************************************************************************
 * Macros
 ************************************************************************************************************/

/************************************************************************************************************
 * Classes implementation
 ************************************************************************************************************/

/************************************************************************************************************/

/***********************************************************************************************************
 * 
 ***********************************************************************************************************/
RBTransformSystem::RBTransformSystem(uint16 uiComponentCount)
{
    RBAssert(uiComponentCount > 0, "Invalid component count");
    m_aLocalMatrices = (RBAlignedMatrix*)_aligned_malloc(uiComponentCount*sizeof(RBAlignedMatrix), 16);
    m_aWorldMatrices = (RBAlignedMatrix*)_aligned_malloc(uiComponentCount*sizeof(RBAlignedMatrix), 16);
    m_aComponents = new RBTransformComponent[uiComponentCount];
    m_aParents = new uint16[uiComponentCount];
    m_uiMatrixCount = uiComponentCount;
    m_uiCurrentMatrix = 0;
}

/***********************************************************************************************************
 * 
 ***********************************************************************************************************/
RBTransformSystem::~RBTransformSystem()
{
    delete[] m_aParents;
    _aligned_free(m_aWorldMatrices);
    _aligned_free(m_aLocalMatrices);
}

/***********************************************************************************************************
 * 
 ***********************************************************************************************************/
RBTransformComponentHandle RBTransformSystem::NewComponent()
{
    RBAssert(m_uiCurrentMatrix < m_uiMatrixCount, "Out of components");
    m_aComponents[m_uiCurrentMatrix].m_idx = m_uiCurrentMatrix;
    return m_uiCurrentMatrix++;
}

/***********************************************************************************************************
 * 
 ***********************************************************************************************************/
void RBTransformSystem::Update()
{

}

/***********************************************************************************************************
 * out = m1 * m2
 ***********************************************************************************************************/
static RBAlignedMatrix MultMatrix(const RBAlignedMatrix& m1, const RBAlignedMatrix& m2)
{
    __m128 v0,v1,v2,v3;

    RBAlignedMatrix out;

    // Select the rows of the appropriate column in M2, and perform the dot product

    // 1st col
    v3 = m2.Col[0];
    v0 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(0,0,0,0));
    v1 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(1,1,1,1));
    v2 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(2,2,2,2));
    v3 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(3,3,3,3));
    v0 = _mm_mul_ps(v0, m1.Col[0]);
    v1 = _mm_mul_ps(v1, m1.Col[1]);
    v2 = _mm_mul_ps(v2, m1.Col[2]);
    v3 = _mm_mul_ps(v3, m1.Col[3]);
    v0 = _mm_add_ps(v0,v2);
    v1 = _mm_add_ps(v1,v3);
    v0 = _mm_add_ps(v0,v1);
    out.Col[0] = v0;

    // 2nd col
    v3 = m2.Col[1];
    v0 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(0,0,0,0));
    v1 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(1,1,1,1));
    v2 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(2,2,2,2));
    v3 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(3,3,3,3));
    v0 = _mm_mul_ps(v0, m1.Col[0]);
    v1 = _mm_mul_ps(v1, m1.Col[1]);
    v2 = _mm_mul_ps(v2, m1.Col[2]);
    v3 = _mm_mul_ps(v3, m1.Col[3]);
    v0 = _mm_add_ps(v0,v2);
    v1 = _mm_add_ps(v1,v3);
    v0 = _mm_add_ps(v0,v1);
    out.Col[1] = v0;

    // 3rd col
    v3 = m2.Col[2];
    v0 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(0,0,0,0));
    v1 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(1,1,1,1));
    v2 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(2,2,2,2));
    v3 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(3,3,3,3));
    v0 = _mm_mul_ps(v0, m1.Col[0]);
    v1 = _mm_mul_ps(v1, m1.Col[1]);
    v2 = _mm_mul_ps(v2, m1.Col[2]);
    v3 = _mm_mul_ps(v3, m1.Col[3]);
    v0 = _mm_add_ps(v0,v2);
    v1 = _mm_add_ps(v1,v3);
    v0 = _mm_add_ps(v0,v1);
    out.Col[2] = v0;

    // 4th col
    v3 = m2.Col[3];
    v0 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(0,0,0,0));
    v1 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(1,1,1,1));
    v2 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(2,2,2,2));
    v3 = _mm_shuffle_ps(v3,v3,_MM_SHUFFLE(3,3,3,3));
    v0 = _mm_mul_ps(v0, m1.Col[0]);
    v1 = _mm_mul_ps(v1, m1.Col[1]);
    v2 = _mm_mul_ps(v2, m1.Col[2]);
    v3 = _mm_mul_ps(v3, m1.Col[3]);
    v0 = _mm_add_ps(v0,v2);
    v1 = _mm_add_ps(v1,v3);
    v0 = _mm_add_ps(v0,v1);
    out.Col[3] = v0;

    return out;
}

/***********************************************************************************************************
 * 
 ***********************************************************************************************************/
static inline void MultMatrices(RBAlignedMatrix* pLocals, RBAlignedMatrix* pWorlds, uint16* pParents,
                                uint16 uiStart, uint16 uiCount)
{
    RBAlignedMatrix* pOffsetLocals = pLocals + uiStart;
    RBAlignedMatrix* pOffsetWorlds = pWorlds + uiStart;
    uint16* pOffsetParents = pParents + uiStart;
    
    static const uint16 ms_lookAhead = 4;

    for (uint16 ui=0; ui<uiCount; ++ui)
    {
        for (uint16 uiLookAhead=0; uiLookAhead < ms_lookAhead; ++uiLookAhead)
        {
            _mm_prefetch((const char*)&pOffsetWorlds[ms_lookAhead-uiLookAhead], _MM_HINT_T0);
            _mm_prefetch((const char*)&pOffsetLocals[ms_lookAhead-uiLookAhead], _MM_HINT_T0);
            _mm_prefetch((const char*)&pWorlds[pOffsetParents[ms_lookAhead-uiLookAhead]], _MM_HINT_T0);
        }

        *pOffsetWorlds++ = MultMatrix(pWorlds[*pOffsetParents++], *pOffsetLocals++);
    }
}

/***********************************************************************************************************
 * 
 ***********************************************************************************************************/
void RBTransformSystem::UpdateMatrices( uint16 uiStart, uint16 uiCount )
{
    MultMatrices(m_aLocalMatrices, m_aWorldMatrices, m_aParents, uiStart, uiCount);
}

/***********************************************************************************************************
 * 
 ***********************************************************************************************************/
void RBTransformSystem::GetWorldMatrix( float4x4& matrix, RBTransformComponentHandle h )
{
    RBAssert(size_t(matrix.GetCol(0)) & 0xFF, "Data must be 16 bytes aligned");
    RBAlignedMatrix& mSrc = m_aWorldMatrices[m_aComponents[h].m_idx];
    _mm_store_ps(matrix.GetCol(0), mSrc.Col[0]);
    _mm_store_ps(matrix.GetCol(1), mSrc.Col[1]);
    _mm_store_ps(matrix.GetCol(2), mSrc.Col[2]);
    _mm_store_ps(matrix.GetCol(3), mSrc.Col[3]);
}

/***********************************************************************************************************
 * 
 ***********************************************************************************************************/
void RBTransformSystem::SetWorldMatrix( const float4x4& matrix, RBTransformComponentHandle h )
{
    RBAssert(size_t(matrix.GetCol(0)) & 0xFF, "Data must be 16 bytes aligned");
    RBAlignedMatrix& mSrc = m_aWorldMatrices[m_aComponents[h].m_idx];
    mSrc.Col[0] = _mm_load_ps(matrix.GetCol(0));
    mSrc.Col[1] = _mm_load_ps(matrix.GetCol(1));
    mSrc.Col[2] = _mm_load_ps(matrix.GetCol(2));
    mSrc.Col[3] = _mm_load_ps(matrix.GetCol(3));

}